// Quaternion.hpp

#ifndef Quaternion_hpp
#define Quaternion_hpp

#include "CrVec4f.h"
#include "CrMaths.h"

//	Simple geometric entity, useful for camera manipulation.

/**

@ingroup Core
@ingroup Math

As quaternions don't lend themselves nicely to PS2 it is almost always better for 
number crunching to build a matrix and use that.

For this reason this is all pretty unoptimised. Pass by reference.

@todo	Virtually all the functions! A lot used to go through a 3x3 matrix. Rubbish.

@todo	Consider tryiung to apply VU0 opcodes


**/

namespace cr
{
	class Quaternion
	{
	public:
		float r,i,j,k;

		// constructors
		Quaternion() : r(1),i(0),j(0),k(0) {}	// safe default constructor
		Quaternion( float _r, float _i, float _j, float _k ) : r(_r),i(_i),j(_j),k(_k) {}
		Quaternion( float ang, const Vec4f& axis );
		Quaternion( const Quaternion &q ) : r(q.r),i(q.i),j(q.j),k(q.k) {}
		Quaternion( const class m44f &);

		//	Quaternion( Vec4f to, Vec4f up );

		// operators
		void operator = ( const class m44f & );

		Quaternion &operator = ( const Quaternion &q ){ r=q.r; i=q.i; j=q.j; k=q.k; return *this; }
		Quaternion operator * ( const Quaternion &q ) const; 	// combine two arbitrary rotations

		Quaternion operator * ( float f ) const						// scale by factor
		{ return Quaternion(r*f, i*f, j*f, k*f); }

		Quaternion &operator += ( const Quaternion &dq )	 	// add a small displacement
		{ r+=dq.r; i+=dq.i; j+=dq.j; k+=dq.k; return *this; }

		Quaternion &operator -= ( const Quaternion &dq )	 	// sub a small displacement
		{ r-=dq.r; i-=dq.i; j-=dq.j; k-=dq.k; return *this; }

		Quaternion operator + ( const Quaternion &dq ) const
		{ return Quaternion(r+dq.r, i+dq.i, j+dq.j, k+dq.k); }

		Quaternion operator - ( const Quaternion &dq ) const
		{ return Quaternion(r-dq.r, i-dq.i, j-dq.j, k-dq.k); }

		Quaternion &operator *= ( const Quaternion &q );

		Quaternion &operator *= ( const float d )
		{
			r *= d;
			i *= d;
			j *= d;
			k *= d;

			return *this;
		}
		Quaternion &operator /= ( const float d )
		{
			if ( d != 0 )
				*this *= (1.0F/d);		// optimization assuming * is faster than / ( not tested )

			return *this;
		}

		// functions
		float Angle() const;		// angle rotated in Radians

		float Magnitude() const 		// length squared (cheaper)
		{
			return r*r + i*i + j*j + k*k;
		}
		float Length() const 			// length should always be 1.0
		{
			return sqrtf(Magnitude());
		}

		void Zero() {r=1.0f;i=j=k=0.0f;}	// zero rotation

		Quaternion &Normalize()		// makes sure length is 1.0
		{
			//*this /= Length();	// 119 Clocks

			// 117 Clocks ( FSQRT = 70/70 )
			float d = 1.0f/sqrtf(r*r + i*i + j*j + k*k);
			r *= d; i *= d;	j *= d;	k *= d;		

			return *this;
		}

		Quaternion &TweenTo(const Quaternion &dest, float factor);	// Move towards dest, factor 0-1
		Quaternion TweenDelta(const Quaternion &dest);

		Quaternion &Invert() // in-place invert
		{ i=-i;j=-j;k=-k; return *this; }		
		Quaternion Inverse() const // return a new inverse
		{ return Quaternion(r,-i,-j,-k); }	

		Vec4f Rotate( Vec4f t ) const;			// rotational transformation of a vector

	};

	//////////////////////////////////////////////////////////////////////////////////////
	/*

	Inline Member Function

	*/

	//////////////////////////////////////////////////////////////////////////////////////
	/*

	Free functions

	*/
	// quaternion negation - equivalent to Invert
	inline Quaternion operator - ( const Quaternion &q)
	{
		return Quaternion(q.r, -q.i, -q.j, -q.k);
	}
	// pre-multiplication by a float
	inline Quaternion operator * ( float d, const Quaternion &q ) 
	{ 
		return Quaternion(d*q.r, d*q.i, d*q.j, d*q.k ); 
	}
	// pre-multiplication by a vector3
	inline Quaternion operator * ( Vec4f t, const Quaternion &q )
	{
		Quaternion q_temp( 0.0f, t.getX(), t.getY(), t.getZ() );
		return q_temp*q;
	}


	//const Quaternion UnitQuat(Vec4f::Zaxis, Vec4f::Yaxis);

}

#endif // Quaternion_hpp







